#include <stdio.h>
#include <stdlib.h>

typedef struct SatMessage {
  double p;
  double prc_satclk;
  double x;
  double y;
  double z;
} SatMessage;

typedef struct SphereCoord {
  double latitude;
  double longitude;
  double height;
} SphereCoord;

SatMessage* LoadDataFromFile(const char* name, int * n) {
       *n = 0;
       FILE *f = fopen("input.dat", "r");
       if (!f) {
           fprintf(stderr, "Error read file input.dat.");
           return NULL;
       }
       SatMessage tmp;
       double dummy;
       while (!feof(f)) {
          fscanf(f, "%lf", &tmp.p);   
          fscanf(f, "%lf", &tmp.x);   
          fscanf(f, "%lf", &tmp.y);   
          fscanf(f, "%lf", &tmp.z);   
          fscanf(f, "%lf", &dummy);   fscanf(f, "%lf", &dummy);fscanf(f, "%lf", &dummy); 
          fscanf(f, "%lf", &tmp.prc_satclk);   
          (*n)++;      
       }
       fclose(f);
       SatMessage* ret = calloc(*n , sizeof (SatMessage));

       f = fopen("input.dat", "r");
       SatMessage *t = ret;
       while (!feof(f)) {
          fscanf(f, "%lf", &t->p);   
          fscanf(f, "%lf", &t->x);   
          fscanf(f, "%lf", &t->y);   
          fscanf(f, "%lf", &t->z); 
          fscanf(f, "%lf", &dummy);   fscanf(f, "%lf", &dummy);fscanf(f, "%lf", &dummy); 
          fscanf(f, "%lf", &t->prc_satclk);   
          t++;
       }
       fclose(f); 
       return ret;

}

SphereCoord LSQCoordinate(SphereCoord* coords, int sz ) {
        SphereCoord ret ;
        ret.longitude = 0;
        ret.latitude = 0;
        ret.height  = 0;
        if (sz >0) {
           ret.longitude = coords->longitude;
           ret.latitude  = coords->latitude;
           ret.height    = coords->height;
           int i = 0;
           for( i = 1; i < sz; ++i) {
              ret.longitude += coords[i].longitude;
              ret.latitude += coords[i].latitude;
              ret.height += coords[i].height;
           }
           ret.longitude /= sz;
           ret.latitude /= sz;
           ret.height /= sz;
        }
        return ret;

}


int main() {

       int  numMsg = 0; 
       int i, j, k, l;
       SatMessage* msgs = LoadDataFromFile("input.dat", &numMsg);
       if (!msgs) {
         return 1;
       }

       if (numMsg < 4) {
          fprintf(stderr, "Insufficient number of messages.\n");
          return 1;
       }
       int combNum = numMsg * (numMsg -1) * (numMsg-2) *(numMsg-3) / 24;
       int c = 0;
       SphereCoord* comb = calloc(combNum,sizeof(SphereCoord));
       for (i = 0; i < numMsg; ++i) {
               for (j = i+1; j < numMsg; ++j) {
                       for (k = j +1; k < numMsg; ++k) {
                               for (l = k +1; l < numMsg; ++l) {
                                       SphereCoord* curr = comb + c;      
                                       if (PerformClosedFormPositionSolution_FromPseuodrangeMeasurements(msgs[i].p,  msgs[j].p,
                                                               msgs[k].p,  msgs[l].p,
                                                               msgs[i].prc_satclk,   msgs[j].prc_satclk,
                                                               msgs[k].prc_satclk,   msgs[l].prc_satclk,    
                                                               msgs[i].x,  msgs[j].x,
                                                               msgs[k].x,  msgs[l].x,    
                                                               msgs[i].y,  msgs[j].y,
                                                               msgs[k].y,  msgs[l].y,    
                                                               msgs[i].z,  msgs[j].z,
                                                               msgs[k].z,  msgs[l].z,    
                                                               &(curr->latitude),
                                                               &(curr->longitude),
                                                               &(curr->height)
                                                               )) {
                                               c++;
                                       }
                               }
                       }
               }
       }
       SphereCoord ret = LSQCoordinate(comb, c);  
       printf("%g %g\n",  ret.longitude*57.295779513, ret.latitude*57.295779513);
       free(comb);
       free (msgs);
       return 0;
}
